// **************************
// I2C Wii Motion Plus 
// **************************
// I2C adress 1: 0xA6 (8bit)    0x53 (7bit)
// I2C adress 2: 0xA4 (8bit)    0x52 (7bit)
// **************
// gyro+acc IMU
// **************
static float gyroZero[3] = {0.0,0.0,0.0};
static float accZero[3]  = {0.0,0.0,0.0};

static float acc_1G = 200;       //this is the 1G measured acceleration (nunchuk)
static float acc_25deg = 85;     //this is the the ACC value measured on x or y axis for a 25deg inclination (nunchuk) = acc_1G * sin(25)
static uint8_t nunchukPresent = 0;



void i2c_WMP_init(uint8_t d) {
  delay(d);
  i2c_rep_start(0xA6 + 0);//I2C write direction => 0
  i2c_write(0xF0); 
  i2c_write(0x55); 
  delay(d);
  i2c_rep_start(0xA6 + 0);//I2C write direction => 0
  i2c_write(0xFE); 
  i2c_write(0x05); 
  delay(d);
  
  if (d>0) {
    uint8_t numberAccRead = 0;
    for(uint8_t i=0;i<100;i++) {
      delay(3);
      if (rawWii() == 0) numberAccRead++; // we detect here is nunchuk extension is available
    }
    if (numberAccRead>25)
      nunchukPresent = 1;
    delay(10);
  }
}

void i2c_WMP_getRawADC() {
  TWBR = ((16000000L / I2C_SPEED) - 16) / 2; // change the I2C clock rate
  i2c_rep_start(0xA4 + 0);//I2C write direction => 0
  i2c_write(0x00);
  i2c_rep_start(0xA4 + 1);//I2C read direction => 1
  for(uint8_t i = 0; i < 5; i++)
    rawADC_WMP[i]=i2c_readAck();
  rawADC_WMP[5]= i2c_readNak();
}


uint8_t rawWii() { //if the WMP or NK are oriented differently, it can be changed here
    i2c_WMP_getRawADC();
    if ( (rawADC_WMP[5]&0x02) == 0x02 && (rawADC_WMP[5]&0x01) == 0 ) {// motion plus data
      Gyro_Vector[PITCH] =  ( ((rawADC_WMP[5]>>2)<<8) + rawADC_WMP[2] );
      Gyro_Vector[ROLL]  = ( ((rawADC_WMP[4]>>2)<<8) + rawADC_WMP[1] );
      Gyro_Vector[YAW]   = -( ((rawADC_WMP[3]>>2)<<8) + rawADC_WMP[0] );
      return 1;
    } else if ( (rawADC_WMP[5]&0x02) == 0 && (rawADC_WMP[5]&0x01) == 0) { //nunchuk data
        Accel_Vector[PITCH] =  ( (rawADC_WMP[3]<<2)        + ((rawADC_WMP[5]>>4)&0x2) );
        Accel_Vector[ROLL]  =  ( (rawADC_WMP[2]<<2)        + ((rawADC_WMP[5]>>3)&0x2) );
        Accel_Vector[YAW]   = -( ((rawADC_WMP[4]&0xFE)<<2) + ((rawADC_WMP[5]>>5)&0x6) );
        return 0;
    } else
      return 2;
}

uint8_t updateWii() {
  static float g[3];
  static float a[3];
  uint8_t axis;
  static float previousGyro_Vector[3] = {0,0,0};
  uint8_t r;
  r=rawWii();
  
  if (currentTime < (neutralizeTime + NEUTRALIZE_DELAY)) {//we neutralize data in case of blocking+hard reset state
    for (axis = 0; axis < 3; axis++) {Gyro_Vector[axis]=0;Accel_Vector[axis]=0;}
    Accel_Vector[YAW] = acc_1G;
  } else {
    if (r == 1) { //gyro
      if (calibratingG>0) {
        for (axis = 0; axis < 3; axis++) {
          if (calibratingG>1) {
            if (calibratingG == 400) g[axis]=0;
            g[axis] +=Gyro_Vector[axis];
            Gyro_Vector[axis]=0;
          } else {
            gyroZero[axis]=(g[axis]+200)/399;
          }
        }
        calibratingG--;
      }
    Gyro_Vector[ROLL]  = Gyro_Vector[ROLL]  - gyroZero[ROLL];
    Gyro_Vector[PITCH] = Gyro_Vector[PITCH] - gyroZero[PITCH];
    Gyro_Vector[YAW]   = Gyro_Vector[YAW]   - gyroZero[YAW];
    Gyro_Vector[ROLL]  = (rawADC_WMP[3]&0x01)     ? Gyro_Vector[ROLL]/5  : Gyro_Vector[ROLL] ;   //the ratio 1/5 is not exactly the IDG600 or ISZ650 specification 
    Gyro_Vector[PITCH] = (rawADC_WMP[4]&0x02)>>1  ? Gyro_Vector[PITCH]/5 : Gyro_Vector[PITCH] ;  //we detect here the slow of fast mode WMP gyros values (see wiibrew for more details)
    Gyro_Vector[YAW]   = (rawADC_WMP[3]&0x02)>>1  ? Gyro_Vector[YAW]/5   : Gyro_Vector[YAW] ;

      //anti gyro glitch, limit the variation between two consecutive readings
      for (axis = 0; axis < 3; axis++) {
        Gyro_Vector[axis] = constrain(Gyro_Vector[axis],previousGyro_Vector[axis]-100,previousGyro_Vector[axis]+100);
        previousGyro_Vector[axis] = Gyro_Vector[axis];
      }
    }
    if (r == 0 ) { //nunchuk or i2c ACC
      if (calibratingA>0) {
        if (calibratingA>1) {
          for (uint8_t axis = 0; axis < 3; axis++) {
            if (calibratingA == 400) a[axis]=0;
            a[axis] +=Accel_Vector[axis];
            Accel_Vector[axis]=0;
          }
        } else {
          accZero[ROLL]  = (a[ROLL]+200)/399;
          accZero[PITCH] = (a[PITCH]+200)/399;
          accZero[YAW]   = (a[YAW]+200)/399+acc_1G; // for nunchuk 200=1G

        }
        calibratingA--;
      } else {
        Accel_Vector[ROLL]  = Accel_Vector[ROLL]  - accZero[ROLL] ;
        Accel_Vector[PITCH] = Accel_Vector[PITCH] - accZero[PITCH];
        Accel_Vector[YAW]   = - (Accel_Vector[YAW]   - accZero[YAW]) ;
      }
    }
  }  
  return r;
}